stb = Besiege.GetBlock("STARTING BLOCK 1")
fs1 = Besiege.GetBlock("FLYING SPIRAL 1")
fs2 = Besiege.GetBlock("FLYING SPIRAL 2")
fs3 = Besiege.GetBlock("FLYING SPIRAL 3")
fs4 = Besiege.GetBlock("FLYING SPIRAL 4")
wh1 = Besiege.GetBlock("WHEEL 1")


global target_altitude
global target_yaw

global last_altitude
global last_pitch
global last_roll
global last_yaw
global speed

target_altitude = 20
target_yaw = 0

last_altitude = 0
last_pitch = 0
last_roll = 0
last_yaw = 0
speed = 8.75

mark = Besiege.CreateMark(Vector3.zero)
def Update():
   
    global target_altitude
    global target_yaw
    
    global last_altitude
    global last_pitch
    global last_roll
    global last_yaw
    global speed

    target_pitch = 0
    if Input.GetKey(KeyCode.UpArrow):
        target_pitch = 15
    if Input.GetKey(KeyCode.DownArrow):
        target_pitch = -15


    if Input.GetKey(KeyCode.H):
        target_yaw -= 0.5
    if Input.GetKey(KeyCode.K):
        target_yaw += 0.5
    if target_yaw > 180 :
        target_yaw -= 360
    if target_yaw < -180 :
        target_yaw += 360
    Besiege.Watch("target yaw",target_yaw)
    
    target_roll = 0
    if Input.GetKey(KeyCode.LeftArrow):
        target_roll = 15
    if Input.GetKey(KeyCode.RightArrow):
        target_roll = -15
    
    if Input.GetKey(KeyCode.U):
        target_altitude += 5*Time.deltaTime
    if Input.GetKey(KeyCode.J):
        target_altitude -= 5*Time.deltaTime

    current_pitch = CurrentAngle(stb.EulerAngles.x)
    speed_pitch = PID(current_pitch,target_pitch,last_pitch,0.2,0.1,0.2)
    last_pitch = current_pitch

    current_yaw = CurrentAngle(stb.EulerAngles.y)
    Besiege.Watch("current yaw",current_yaw)
    speed_yaw = PID(current_yaw,target_yaw,last_yaw,0.3,0.1,0.2)
    last_yaw = current_yaw
    
    current_roll = CurrentAngle(stb.EulerAngles.z)
    speed_roll = PID(current_roll,target_roll,last_roll,0.2,0.1,0.2)
    last_roll = current_roll
    
    current_altitude = CurrentAngle(stb.Position.y)
    speed_altitude = PID(current_altitude,target_altitude,last_altitude,0.3,0.3,0.5)*1.5
    last_altitude = current_altitude
 
    fs1_speed = speed + speed_pitch + speed_roll + speed_altitude
    fs2_speed = speed + speed_pitch - speed_roll + speed_altitude
    fs3_speed = speed - speed_pitch + speed_roll + speed_altitude
    fs4_speed = speed - speed_pitch - speed_roll + speed_altitude
    
    fs1.SetSliderValue("FLYING SPEED", fs1_speed/10)
    fs2.SetSliderValue("FLYING SPEED", fs2_speed/10)
    fs3.SetSliderValue("FLYING SPEED", fs3_speed/10)
    fs4.SetSliderValue("FLYING SPEED", fs4_speed/10)

    wh1.SetSliderValue("SPEED", speed_yaw)

    mark.Move(Vector3(stb.Position.x + 5 * Mathf.Sin(target_yaw * Mathf.Deg2Rad),target_altitude,stb.Position.z + 5 * Mathf.Cos(target_yaw * Mathf.Deg2Rad)))

def PID(current_angle,target_angle,last_angle,p,i,d,p_max = 2,i_max = 1,d_max = 1,s_max = 1):
        
    t = Time.deltaTime

    error = target_angle - current_angle
    if error > 180 or error < -180 :
        error = -target_angle + current_angle
        
    PTerm = error * p
    PTerm = Clamp(PTerm,-p_max,p_max);

    ITerm = i * error * t + i * (target_angle -last_angle) * t
    ITerm = Clamp(ITerm,-i_max,i_max);
        
    try:
        DTerm = d * (current_angle - last_angle) / t
    except ZeroDivisionError as e:
        DTerm = 0

    DTerm = Clamp(DTerm,-d_max,d_max)          

    speed = PTerm + ITerm - DTerm
    speed = Clamp(speed,-s_max,s_max)
    return speed

def Clamp(value,Min,Max):
    if value > Max :
        value = Max
    if value < Min :
        value = Min
    return value

def CurrentAngle(angle):
    if angle > 180 :
        angle -= 360
    return angle
